#include "math.h"

#include "adrc.h"
#include "algorism.h"
/**
  * @brief  fhan最速综合函数
  * @note 根据输入信号的大小和变化率自适应地调整输出信号的幅值和频率，使得输出信号能够最快地跟踪输入信号，同时抑制噪声和干扰的影响
  *
  */
static float fhan(float x1, float x2, float r, float h)
{
	float d, a0, y, a1, a2, sy, a, sa;	
	d  = r * square(h);
	a0 = h * x2;
	y  = x1 + a0;
	a1 = sqrt(d * (d + 8 * fabs(y)));
	a2 = a0 + fsgn(y) * (a1 - d) / 2.0f;
	sy = (fsgn(y + d) - fsgn(y - d)) / 2.0f;
	a = (a0 + y - a2) * sy + a2;
	sa = (fsgn(a + d) - fsgn(a - d)) / 2.0f;
	return -r * (a / (1.0f * d) - fsgn(a)) * sa - r * fsgn(a);
}

/**
  * @brief  非线性fal函数
  * @retval float 
  * @note y = e / delta^(1-alpha) when fabs(e) < fabs(delta)
  * 	  y = sign(e) * fabs(e)^alpha when fabs(e) > fabs(delta)
  */
float Fal(float e, float alpha, float delta)
{
	if (fabs(e) <= fabs(delta))
		return e / powf(fabs(delta), 1 - alpha);
	else
		return fsgn(e) * powf(fabs(e), alpha);
}

/**
 * @brief TD微分器初始化
 * @param [0]TD结构体
 * @param [1]速度因子
 * @param [2]积分步长
 * @param [3]积分步长扩大系数
 * @param [4]周期信号下限
 * @param [5]周期信号上限
 * @retval void
 * @note 若处理非周期信号，设置上下限均为0 
 */
void TDInitialize(TD* td, float r, float h0, float N, float min, float max)
{
	td->h = h0 * N;
	td->r = r;
	td->max = max;
	td->min = min;
}

/**
 * @brief TD微分器计算
 * @param [0]TD结构体
 * @param [1]目标量
 * @retval void
 */
void TDUpdate(TD* td, float target)
{
	float x1_delta = td->x1 - target;
	
	if(td->min != td->max)
		x1_delta = AngleLimit(x1_delta, td->min, td->max);
	
	td->x1 += td->h * td->x2;
	td->x2 += td->h * fhan(x1_delta, td->x2, td->r, td->h);
	
	if(td->min != td->max)
		td->x1 = AngleLimit(td->x1, td->min, td->max);	
}

/**
 * @brief ESO扩张状态观测器初始化
 *  一般取b_01 = 1/h b_02 = 1/(3*h*h) b_03 = 1 / (20*h*h*h)
 * @param [0]ESO结构体
* @param [1]积分步长，若b取为0，则取消控制量修正，将电机控制量囊括在外部扰动中
 * @param [2]x_dot = Ax + Bu中控制矩阵u关于一阶量的因子
 * @param [3]z1_dot = h * (z2 - b_01 * error)
 * @param [4]z2_dot = h * (z3 - b_02 * Fal(error, 0.5, h) + b*u)
 * @param [5]z3_dot = -h * (b_03 * Fal(error, 0.25, h))
 * @param [6]若一阶信号为周期信号，该参数给为周期限幅下限，否则给0
 * @param [7]若一阶信号为周期信号，该参数给为周期限幅上限，否则给0
 * @retval void
 */
void ESOInitialize(ESO* eso, float h, float b, float b_01, float b_02, float b_03, float z3_limit, float min, float max)
{
	eso->h = h;
	eso->b = b;
	eso->beta_01 = b_01;
	eso->beta_02 = b_02;
	eso->beta_03 = b_03;
	eso->z3_limit = z3_limit;
	eso->max = max;
	eso->min = min;
}

/**
 * @brief ESO 扩张状态观测器更新
 * @param [0] ESO结构体
 * @param [1] 受控目标1阶返回观测值
 * @param [2] 给予控制目标先验控制量
 * @retval void
 */
void ESOUpdate(ESO* eso, float feedback, float control_val)
{
	float e = eso->z1 - feedback;
	if(eso->min != eso->max)
		e = AngleLimit(e, eso->min, eso->max);
    eso->z1 += eso->h * (eso->z2 - eso->beta_01 * e);
	if(eso->b != 0)	
		eso->z2 += eso->h * (eso->z3 - eso->beta_02 * Fal(e, 0.5, eso->h) + eso->b * control_val);
	else
		eso->z2 += eso->h * (eso->z3 - eso->beta_02 * Fal(e, 0.5, eso->h));
    eso->z3 -= eso->h * (eso->beta_03 * Fal(e, 0.25, eso->h));	
	eso->z3 = AbsLimiter(eso->z3, eso->z3_limit);
	
	if(eso->min != eso->max)
		eso->z1 = AngleLimit(eso->z1, eso->min, eso->max);	
}

/// @brief 线性组合器初始化
/// @param esf ESF结构体
/// @param k_0 误差积分放大系数
/// @param k_1 误差放大系数
/// @param k_2 误差微分放大系数
/// @param e_0_max 误差积分上限
/// @param output_limit 输出限幅
void LESFInitialize(ESF* esf, float k_0, float k_1, float k_2, float e_0_max, float output_limit)
{
	esf->k_0 = k_0;
	esf->k_1 = k_1;
	esf->k_2 = k_2;
	esf->e_0_max = e_0_max;
	esf->output_limit = output_limit;
}

/**
 * @brief 线性组合器计算更新
 * @param [0]esf结构体
 * @param [1]一阶误差输入
 * @param [2]二阶误差输入
 * @retval output_val
 */
float LESFUpdate(ESF* esf, float e_1, float e_2)
{
	esf->e_0 += e_1;
	esf->e_1 = e_1;
	esf->e_2 = e_2;
	esf->e_0 = AbsLimiter(esf->e_0, esf->e_0_max);
	esf->output = esf->e_0 * esf->k_0 + e_1 * esf->k_1 + e_2 * esf->k_2;
	esf->output = AbsLimiter(esf->output, esf->output_limit);
	return esf->output;
}


/// @brief LADRC结构体初始化
/// @param adrc adrc结构体
/// @param td_init_val td环节参数 td_init_val[3]={r, h0, N}
/// @param esf_init_val esf环节参数（线性esf初始化） esf_init_cal[5]={k_0, k_1, k_2, e_0_max, output_limit}
/// @param eso_init_val eso环节参数 eso_init_cal[6]={h, b, beta_01, beta_02, beta_03, z3_limit}
/// @param min 若处理的信号为周期函数，min为周期下限
/// @param max 若处理的信号为周期函数，max为周期上限，周期在adrc不同环节内统一，若为非周期信号，请输入任意min = max
/// @note 严禁修改结构体内变量顺序，给予参数请严格按照注释给定
void LADRCInitialize(ADRC* adrc, float* td_init_val, float* lesf_init_val, float* eso_init_val, float min, float max)
{
	TDInitialize(&adrc->td, td_init_val[0], td_init_val[1], td_init_val[2], min, max);
	LESFInitialize(&adrc->esf, lesf_init_val[0], lesf_init_val[1], lesf_init_val[2], lesf_init_val[3], lesf_init_val[4]);
	ESOInitialize(&adrc->eso, eso_init_val[0], eso_init_val[1], eso_init_val[2], eso_init_val[3], eso_init_val[4], eso_init_val[5], min, max);
	adrc->limit_max = max;
	adrc->limit_min = min;
}

/// @brief 线性自抗扰计算
/// @param adrc 自抗扰控制计算所需结构体
/// @param target 控制目标期望值
/// @param feedback 控制目标反馈值
void LADRCUpdate(ADRC* adrc, float target, float feedback)
{
	//input transection
	TDUpdate(&adrc->td, target);
	//calculate control value without disturbance compensation
	adrc->u_0 = LESFUpdate(&adrc->esf, AngleLimit(adrc->td.x1 - adrc->eso.z1, adrc->limit_min, adrc->limit_max), adrc->td.x2 - adrc->eso.z2);
	//estimate disturbance compensation 
	if(adrc->eso.b == 0)
		adrc->u = adrc->u_0;
	else
		adrc->u = (adrc->u_0 - adrc->eso.z3) / (adrc->eso.b * 1.0f);
	//update extensional state observer
	ESOUpdate(&adrc->eso, feedback, adrc->u);
}